from machine import I2C,UART,Pin
from  Sentry3  import *
import time
import random

# 等待Sentry3完成操作系统的初始化。此等待时间不可去掉，避免出现Sentry3尚未初始化完毕主控器已经开发发送指令的情况
time.sleep(2)

# 选择UART或者I2C通讯模式，Sentry3出厂默认为I2C模式，短按模式按键可以切换
# 4种UART通讯模式：UART9600（标准协议指令），UART57600（标准协议指令），UART115200（标准协议指令），Simple9600（简单协议指令），
# 参看“简单协议指令”
# https://tosee.readthedocs.io/zh/latest/Common/Protocol/index.html#section-6
#########################################################################################################
# port = UART(2,rx=Pin(16),tx=Pin(17),baudrate=9600)
port = I2C(1,scl=Pin(22),sda=Pin(21),freq=400000)

# Sentry3通讯地址：0x60。如果I2C总线挂接多个设备，请避免出现地址冲突
Sentry3 = Sentry3(0x60)

err = Sentry3.begin(port)
print("Sentry3.begin: 0x%x"% err)


# 参数设置
# 利用随机数，决定色块算法参数组的数量，并且从6组预设的参数内依次取出相同组数的参数赋值给算法
#########################################################################################################
# 设置每种颜色的结果数量，算法默认设置为1
Sentry3.VisionSetMode(sentry3_vision_e.kVisionBlob,1)
index = (random.randint(1, 6))
# 设置算法参数组的数量，算法默认设置为1
Sentry3.SetParamNum(sentry3_vision_e.kVisionBlob,index)
print("random= %d"%index)
for i in range(1, index+1):
    if (i == 1):
        Sentry3.SetParam(sentry3_vision_e.kVisionBlob,[0, 0, 6, 8, color_label_e.kColorYellow],1)
    if (i == 2):
        Sentry3.SetParam(sentry3_vision_e.kVisionBlob,[0, 0, 6, 8, color_label_e.kColorBlue],2)
    if (i == 3):
        Sentry3.SetParam(sentry3_vision_e.kVisionBlob,[0, 0, 10, 10, color_label_e.kColorGreen],3)
    if (i == 4):
        Sentry3.SetParam(sentry3_vision_e.kVisionBlob,[0, 0, 10, 10, color_label_e.kColorRed],4)
    if (i == 5):
        Sentry3.SetParam(sentry3_vision_e.kVisionBlob,[0, 0, 12, 9, color_label_e.kColorWhite],5)
    if (i == 6):
        Sentry3.SetParam(sentry3_vision_e.kVisionBlob,[0, 0, 12, 9, color_label_e.kColorBlack],6)
    time.sleep(0.1)


 
# 1、色块检测算法可以同时设置6组参数，如果每组参数对应1种颜色，则可以同时检测6种颜色的色块；
# 2、算法默认每种颜色只输出1个结果，每种颜色最多5个结果，但总的输出结果不超过25个
# 3、算法介绍参考
# https://tosee.readthedocs.io/zh/latest/Sentry2/Vision/index.html#chapter-vision-blob-index
# 4、正常使用时，应由主控器发送指令控制Sentry2算法的开启与关闭，而非通过摇杆手动进行操作；
# 5、满足特定限制条件下，Sentry3可以并行运行多个识别算法
err = Sentry3.VisionBegin(sentry3_vision_e.kVisionBlob)
print("Sentry3.VisionBegin(sentry3_vision_e.kVisionBlob):0x%x"% err)


while True:
    # Sentry3不主动返回检测识别结果，需要主控板发送指令进行读取。读取的流程：首先读取识别结果的数量，接收到指令后，Sentry3会刷新结果数据，如果结果数量不为零，那么主控再发送指令读取结果的相关信息。请务必按此流程构建程序。
    obj_num = Sentry3.GetValue(sentry3_vision_e.kVisionBlob, sentry_obj_info_e.kStatus)
    
    if obj_num:
        print("Totally %d blobs: "%( obj_num ))
        for i in range(1,obj_num+1):
            x = Sentry3.GetValue(sentry3_vision_e.kVisionBlob, sentry_obj_info_e.kXValue, i)
            y = Sentry3.GetValue(sentry3_vision_e.kVisionBlob, sentry_obj_info_e.kYValue, i)
            w = Sentry3.GetValue(sentry3_vision_e.kVisionBlob, sentry_obj_info_e.kWidthValue, i)
            h = Sentry3.GetValue(sentry3_vision_e.kVisionBlob, sentry_obj_info_e.kHeightValue, i)

            if (Sentry3.GetValue(sentry3_vision_e.kVisionBlob,sentry_obj_info_e.kLabel,i)==color_label_e.kColorBlack):
                print("blob%d: Black , "%i, end='')
            if (Sentry3.GetValue(sentry3_vision_e.kVisionBlob,sentry_obj_info_e.kLabel,i)==color_label_e.kColorWhite):
                print("blob%d: White , "%i, end='')
            if (Sentry3.GetValue(sentry3_vision_e.kVisionBlob,sentry_obj_info_e.kLabel,i)==color_label_e.kColorRed):
                print("blob%d: Red   , "%i, end='')
            if (Sentry3.GetValue(sentry3_vision_e.kVisionBlob,sentry_obj_info_e.kLabel,i)==color_label_e.kColorGreen):
                print("blob%d: Green , "%i, end='')
            if (Sentry3.GetValue(sentry3_vision_e.kVisionBlob,sentry_obj_info_e.kLabel,i)==color_label_e.kColorBlue):
                print("blob%d: Blue  , "%i, end='')
            if (Sentry3.GetValue(sentry3_vision_e.kVisionBlob,sentry_obj_info_e.kLabel,i)==color_label_e.kColorYellow):
                print("blob%d: Yellow, "%i, end='')
            
            print("x=%d, y=%d, w=%d, h=%d"%( x, y, w, h))
            time.sleep(0.2)
        print("\n")
